import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import sys

sys.path.append('/home/cranebot/ros2_ws/src/cranebot/gpio')
from jy901 import JY901

class ImuReader(Node):

    def __init__(self):
        super().__init__('imu_reader')
        self.publisher_ = self.create_publisher(Imu, '/cranebot/imu', 10)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.imu = JY901()
        self.roll_init, self.pitch_init, self.yaw_init = self.imu.calc_angle_value()
    
    def timer_callback(self):
        msg = Imu()
        try:
            x_acc, y_acc, z_acc = self.imu.calc_accelerate_value()
            x_ang_vel, y_ang_vel, z_ang_vel = self.imu.calc_angle_vel_value()
            q_0, q_1, q_2, q_3 = self.imu.calc_quaternion_value()
        except:
            self.get_logger().info("Reading Error")
            return
        roll, pitch, yaw = self.imu.calc_angle_value()
        msg.angular_velocity.x = roll-self.roll_init
        msg.angular_velocity.y = pitch-self.pitch_init
        msg.angular_velocity.z = yaw-self.yaw_init
        msg.linear_acceleration.x = x_acc
        msg.linear_acceleration.y = y_acc
        msg.linear_acceleration.z = z_acc
        msg.orientation.x = q_0
        msg.orientation.y = q_1
        msg.orientation.z = q_2
        msg.orientation.w = q_3
        self.publisher_.publish(msg)
        #self.get_logger().info(str([roll, pitch, yaw]))

def main(args=None):
    rclpy.init(args=args)
    imu_reader = ImuReader()
    rclpy.spin(imu_reader)
    imu_reader.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

